clear; clc
%%
gain=-3.0e3; tau=10.0; order=8;  bw3dB=6.0;%20.35;
tune(gain, tau, order, bw3dB);
plant = oscillator(gain, tau);
flter = zifilter(order, bw3dB);

%%
function tune(gain, tau, order, bw3dB)
    global Kp Ki Kd dKp dKi dKd isquit;
    Kp= -2e-3; Ki=-2e-3; Kd=0.0; dKp=1e-3; dKi=1e-3; dKd=1e-4; isquit=false;
    
    g = oscillator(gain, tau);
    f = zifilter(order, bw3dB);

    fig = figure;
    ax = axes();
    fig.('KeyPressFcn') =  @(obj, event) change_param(obj, event, ax, g, f);
    update_plot(ax, g, f);
    
    while ~isquit
        waitforbuttonpress
    end
    close(fig);
end

function change_param(~, event, ax, g, f)
    global Kp Ki Kd dKp dKi dKd isquit;
    switch event.Key
        case 'u'
            Kp=Kp+dKp;
            update_plot(ax, g, f);
        case 'i'
            Ki=Ki+dKi;
            update_plot(ax, g, f);
        case 'o'
            Kd=Kd+dKd;
            update_plot(ax, g, f);

        case 'j'
            Kp=Kp-dKp;
            update_plot(ax, g, f);
        case 'k'
            Ki=Ki-dKi;
            update_plot(ax, g, f);
        case 'l'
            Kd=Kd-dKd;
            update_plot(ax, g, f);
            
        case 'q'
            isquit = true;

    end
end

function update_plot(ax, g, f)
    global Kp Ki Kd;
    
    q1f = Qd1f(Kp, Ki, Kd, g, f);
    q1p = Qd1phi(Kp, Ki, Kd, g, f);
    q2f = Qd2f(Kp, Ki, Kd, g, f);
    q2p = Qd2phi(Kp, Ki, Kd, g, f);
    
    fList = logspace(-2, 2, 100);
    mag1f = bode(q1f, fList);
    mag1p = bode(q1p, fList);
    mag2f = bode(q2f, fList);
    mag2p = bode(q2p, fList);
    
    [yf, tf]=step( q1f );
    [yp, tp]=step( q1p );
    
    lpsd_f = @(f) 55e-6./f + 5.5e-6;
    psd_phi = 1e-2;    
    lpsd_phi = @(f) 0.0 .* f + psd_phi;

    deltaF = 30e-3;

    maxT = 10.0; nT = 1000; seed = 1;
    [yTot, yF, yPhi, tList, noiseF, noisePhi] = noiseSim(maxT, nT, lpsd_f, lpsd_phi, g, f, seed);
    
    %%
    subplot(3, 3, 1)
    yyaxis left;
    loglog(fList, squeeze(mag1f) ); grid on; ylabel('freq-freq'); xlabel('frequency (Hz)');
    yyaxis right;
    loglog(fList, squeeze(mag2f) ); grid on; ylabel('phase-freq (Hz/deg)');
    title(sprintf('Kp=%5.3f m, Ki=%5.3f m, Kd=%5.3f m', Kp*1e3, Ki*1e3, Kd*1e3)); legend('freq1', 'freq2');
    %%
    subplot(3, 3, 2)
    yyaxis left;
    loglog(fList, squeeze(mag1p) ); grid on; ylabel('freq-phase (deg/Hz)'); xlabel('frequency Hz')
    yyaxis right;
    loglog(fList, squeeze(mag2p) ); legend('phi1', 'phi2'); grid on; ylabel('phase-phase (1)'); xlabel('frequency Hz');
    %%
    subplot(3, 3, 4)
    loglog(fList, 1e6*( squeeze(mag1f).*lpsd_f(fList.')  + squeeze(mag2f).*psd_phi), 'k.-', ...
        fList, 1e6*( squeeze(mag1f).*lpsd_f(fList.')), 'b-', ...
        fList, 1e6*( squeeze(mag2f).*psd_phi), 'r-'); 
    grid on; ylabel('Frequency PSD ({\mu}Hz \cdot Hz^{-1/2})'); ylim([1 1e4]); xlabel('frequency (Hz)'); legend('total noise', 'frequency noise', 'phase noise');
    %%
    subplot(3, 3, 5)
    loglog(fList, squeeze(mag1p).*lpsd_f(fList.')  + squeeze(mag2p).*psd_phi, 'k.-', ...
        fList, squeeze(mag1p).*lpsd_f(fList.'), 'b-', ...
        fList, squeeze(mag2p).*psd_phi, 'r-'); 
    grid on; ylabel('Phase PSD (deg \cdot Hz^{-1/2})'); ylim([1e-4 1]); xlabel('frequency (Hz)'); legend('total noise', 'frequency noise', 'phase noise');
    %%
    subplot(3, 3, 7)
    plot(tf, -deltaF*yf*1e3)
    grid on; xlim(minmax(tf.')); xlabel('time (s)'); ylabel('frequency (mHz)');
    %%
    subplot(3, 3, 8)
    plot(tp, deltaF*yp)
    grid on; xlim(minmax(tp.')); xlabel('time (s)'); ylabel('phase (deg)');
    %%
    subplot(3, 3, 3)
    yyaxis left; plot(tList, noisePhi); ylabel('phase noise (deg)');ylim([-1 1]);
    yyaxis right; plot(tList, yPhi*1e3); ylabel('frequency response (mHz)');ylim([-5 5]);
    grid on; xlim(minmax(tList)); xlabel('time (s)');
    %%
    subplot(3, 3, 6)
    plot(tList, yF*1e3, tList, -noiseF*1e3); ylabel('frequency noise (mHz)');
    grid on; xlim(minmax(tList)); xlabel('time (s)');
    %%
    subplot(3, 3, 9)
    plot(tList, yF*1e3, 'r-', tList, yPhi*1e3, 'b-', tList, yTot*1e3, 'k.-'); ylabel('frequency noise (mHz)'); ylim([-5 5]);
    grid on; xlim(minmax(tList)); xlabel('time (s)');
end
